% Program to orient Mars 2020 cores using rover data
% Benjamin P. Weiss (MIT)
% Requires functions Projection3.m, signedAngleTwo3DVectors.m, and WATSON_mark4.m
% Compatible with at least MATLAB R2023a; Requires Navigation Toolbox and Mapping Toolbox
% v25; 11/29/2023

clear all

%USER INPUTS:
markimage = 1; %set markimage = 1 to mark WATSON image with core roll
corenames = ["Roubion", "Montdenier", "Montagnac", "Salette", "Coulettes", "Robine", "Malay", "Hahonih", "Atsah",...
 "Swift Run", "Skyland", "Hazeltop", "Bearwallow", "Shuyak", "Mageik", "Kukaklek", "Melyn", "Otis Peak", "Pilot Mountain", "Lefroy Day"];  %names of core targets to process
WATSON_in_path = ['/Users/User/image_in/']; %path to input raw WATSON images to be annotated for roll
WATSON_out_path = ['/Users/User/image_out/']; %path to output annotated WATSON images
WATSON_out_filetype = ['eps']; %filetype of output annotated WATSON image
%-----------------------------    
%CORE ORIENTATION METADATA:

for i = 1:size(corenames,2)
    core_target = corenames(1,i)
    %Core orientation information (rover metadata)
    if strcmp(core_target,'Roubion') == 1
        bQll1 = [0.332886, -0.0174924, -0.00616092, 0.942785]; %bQll quaternion during drilling (scalar-vector)
        cdQb =  [0.665264,  0.233161,  -0.661495,   0.255898]; %Coring Drill x-axis in Rover Mechanical frame
        bQll2 = [0.332934, -0.0148229, -0.00733084, 0.942805]; %bQll quaternion during WATSON imaging (scalar-vector)
        wQb = [0.667723, 0.237418, -0.658963, 0.252084];       %quaternion rotating from WATSON to Rover Mechanical frame
        WATSON_name = ['SIF_0161_0681240940_632FDR_N0060000SRLC00702_0000LMJ01']; %name of WATSON image to be annotated with roll (png file)
        mcount = 13692;                                        %FOCUS_POSITION_COUNT metadata for determining scale bar for WATSON image 
        wat_z_dim = 1648;                                      %image scale (z-dimension) of WATSON images (pixels)
    elseif strcmp(core_target, 'Montdenier') == 1
        bQll1 = [0.696481, 0.0183586, -0.00168854, -0.717339]; 
        cdQb = [0.705714,  0.2091890,   -0.664503,  0.129009];    
        bQll2 = [0.696524, 0.0163777, -0.00351771, -0.717338];  
        wQb = [0.706087, 0.213611, -0.663116, 0.126843];
        WATSON_name = ['SIF_0188_0683643364_351FDR_N0070000SRLC00678_0000LMJ01'];
        mcount = 13854;
        wat_z_dim = 1648;
    elseif strcmp(core_target,'Montagnac') == 1
        bQll1 = [0.696493, 0.0181555, -0.0019524, -0.717331];   
        cdQb = [0.730214, 0.260758, -0.623407, 0.100777];   
        bQll2 =  [0.696528, 0.0164346, -0.00334345, -0.717334];  
        wQb = [0.729438, 0.26773, -0.620981, 0.103072];
        WATSON_name = ['SIF_0195_0684255577_390FDR_N0070000SRLC00701_0000LMJ01'];
        mcount = 13656;
        wat_z_dim = 1648;
    elseif strcmp(core_target,'Salette') == 1
        bQll1=[0.8161970, -0.0570105, 0.0018349, 0.5749510];  
        cdQb =[0.7611450, 0.1932350, -0.6043060, 0.1346610]; 
        bQll2 = [0.816367, -0.0553839, -0.000861533, 0.574871];
        wQb = [0.761644, 0.197503, -0.60257, 0.133416];
        WATSON_name = ['SIF_0257_0689756802_085FDR_N0080000SRLC00654_0000LUJ01'];
        mcount = 13704;
        wat_z_dim = 1584    ;
    elseif strcmp (core_target, 'Coulettes') == 1
        cdQb = [0.669276, 0.377496, -0.573508, 0.283997]; 
        bQll1 = [0.816187, -0.0570269, 0.00166007, 0.574964]; 
        bQll2 = [0.816343, -0.0553775, -0.000576532, 0.574906]; 
        wQb = [0.669666, 0.383208, -0.570312, 0.281855]; 
        WATSON_name = ['SIF_0267_0690661712_886FDR_N0080000SRLC00701_0000LMJ01'];
        mcount = 13710;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Robine") == 1
        bQll1 = [0.428339, 0.0740838, 0.026352,-0.900191]; 
        cdQb = [0.664618, 0.163109, -0.706837, 0.179054]; 
        bQll2 = [0.428458, 0.0711518 ,0.0249948, -0.900409];  
        wQb = [0.666062, 0.167862, -0.704872, 0.177028]; 
        WATSON_name = ['SIF_0293_0692957683_398FDR_N0090000SRLC00643_0000LMJ02'];
        mcount = 13824;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Malay") == 1
        bQll1 = [0.945249, 0.00162226, 0.00858244, 0.326233]; 
        cdQb = [0.698638,0.150402, -0.67758, 0.173692];
        bQll2 = [0.945265, 0.00189065, 0.00584646, 0.326245];  
        wQb = [0.700578, 0.153594, -0.675732, 0.17025];  
        WATSON_name = ['SIF_0337_0696859111_167FDR_N0090276SRLC00636_0000LMJ01'];
        mcount = 13806;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Hahonih") == 1
        bQll1 = [0.1500300, 0.0144798, -0.0209876, -0.9883530]; 
        cdQb = [0.5911830, 0.1944370, -0.7818390, 0.0377382]; 
        bQll2 = [0.1500950, 0.0115230, -0.0213943, -0.9883730];  
        wQb = [0.5908020, 0.1961150, -0.7817180, 0.0375390];  
        WATSON_name = ['SIF_0370_0699795188_894RAS_N0110108SRLC00663_0000LMJ01'];
        mcount = 13878;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Atsah") == 1
        bQll1 = [0.150025, 0.0148175, -0.020817, -0.988352]; 
        cdQb = [0.58264, 0.235836, -0.76571, 0.136382]; 
        bQll2 = [0.1501010, 0.0114020, -0.0207122, -0.9883880];  
        wQb = [0.5801300 , 0.23807000, -0.7670220 , 0.1358290];
        WATSON_name = ['SIF_0373_0700065505_402RAS_N0110108SRLC00657_0000LMJ01'];
        mcount = 13866;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Swift Run") == 1
        bQll1 = [0.990075, 0.0411996, 0.11617, 0.0675102]; 
        cdQb = [0.69366, 0.217854, -0.646779, 0.230331]; 
        bQll2 = [0.990376, 0.0408681, 0.113655, 0.067592];  
        wQb = [0.695267, 0.221652, -0.645046, 0.2266930];
        WATSON_name = ['SIF_0485_0709994755_121FDR_N0261004SRLC00730_0000LMJ01'];
        mcount = 13734;
        wat_z_dim = 1584;
    elseif strcmp (core_target, "Skyland") == 1
        bQll1 = [0.990082, 0.041088, 0.116157, 0.0675021]; 
        cdQb = [0.719608, 0.136234, -0.656689, 0.1799]; 
        bQll2 = [0.990385, 0.0409456, 0.113539, 0.0676066];  
        wQb = [0.721544, 0.138711, -0.655143, 0.175847];
        WATSON_name = ['SIF_0485_0709996024_867FDR_N0261004SRLC00730_0000LMJ01'];
        mcount = 13770;
        wat_z_dim = 1584;
    elseif strcmp (core_target, "Hazeltop") == 1
        bQll1 = [0.583193, -0.0843587, 0.0916021, -0.802732]; 
        cdQb = [0.63817, 0.251639, -0.683091, 0.250607]; 
        bQll2 = [0.582866, -0.0866888, 0.0898041, -0.802924];  
        wQb = [0.631282, 0.225406, -0.69697, 0.254771];
        WATSON_name = ['SIF_0505_0711773216_015RAS_N0261222SRLC00730_0000LMJ01'];
        mcount = 13698;
        wat_z_dim = 1584;
    elseif strcmp (core_target, "Bearwallow") == 1
        bQll1 = [0.583193, -0.0844151, 0.0915364, -0.802733]; 
        cdQb = [0.643634, 0.250863, -0.671522, 0.26807]; 
        bQll2 = [0.582882, -0.0866339, 0.0897449, -0.802925];  
        wQb = [0.647031, 0.254311, -0.668715, 0.263623];
        WATSON_name = ['SIF_0511_0712314955_726FDR_N0261222SRLC00773_0000LMJ01'];
        mcount = 13614;
        wat_z_dim = 1584;
    elseif strcmp (core_target, "Shuyak") == 1
        bQll1 = [0.0812557, 0.0365753, 0.0139382, -0.995924]; 
        cdQb = [0.733837, 0.222457, -0.586081, 0.261735]; 
        bQll2 = [0.081236, 0.0347169, 0.013902, -0.995993];  
        wQb = [0.736514, 0.228713, -0.58195, 0.258015]; 
        WATSON_name = ['SIF_0570_0717551784_765FDR_N0290000SRLC00750_0000LMJ01'];
        mcount = 13632;
        wat_z_dim = 1584;
    elseif strcmp (core_target, "Mageik") == 1
        bQll1 = [0.081253, 0.0358325, 0.0139146, -0.995952];   
        cdQb = [0.0404725, -0.825806, -0.0571939, -0.559585]; 
        bQll2 = [0.0812331, 0.0340338, 0.0137684, -0.996019];  
        wQb = [0.0394707, -0.827204, -0.0578265, -0.557522];   
        WATSON_name =['SIF_0577_0718170754_562FDR_N0290000SRLC00701_0000LMJ01']; 
        mcount = 13632; 
        wat_z_dim = 1584;
    elseif strcmp (core_target, "Kukaklek") == 1
        bQll1 = [0.599375, 0.0166575, 0.0456629, -0.798991];
        cdQb = [0.666485, 0.242253, -0.660351, 0.247076];
        bQll2 = [0.599385, 0.0143712, 0.0438615, -0.799129];
        WATSON_name =['SIF_0614_0721455441_734FDR_N0301172SRLC00643_0000LMJ01'];
        wQb = [0.668613, 0.246199, -0.658112, 0.243375];
        mcount = 13872;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Melyn") == 1
        bQll1 = [0.763689, 0.0224693, -0.0205009, -0.644867];
        cdQb = [0.726386, 0.162145, -0.648814, 0.158471];
        bQll2 = [0.763752, 0.0199443, -0.022929, -0.644793]; 
        wQb = [0.729311, 0.165953, -0.645422, 0.154905];
        WATSON_name =['SIF_0747_0733259668_218FDR_N0370000SRLC00748_0000LMJ01'];
        mcount = 13632;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Otis Peak") == 1
        bQll1 = [0.59263, 0.07454, -0.03873, -0.80108];
        cdQb = [0.68423, 0.09286, -0.70742, 0.15087];
        bQll2 = [0.5929, 0.07177, -0.04055, -0.80104];
        wQb = [0.68605, 0.09245, -0.70602, 0.14942];
        WATSON_name =['SIF_0822_0739913201_500FDR_N0400132SRLC00660_0000LMJ01'];
        mcount = 13752;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Pilot Mountain") == 1
        bQll1 = [0.65841, 0.02671, 0.03392, -0.75142];
        cdQb = [0.65366, 0.21847, -0.69072, 0.21887];
        bQll2 = [0.65848, 0.02449, 0.03167, -0.75154];
        wQb = [0.6561, 0.22214, -0.68831, 0.21545];
        WATSON_name =['SIF_0879_0744977905_546FDR_N0430000SRLC00743_0000LMJ01'];
        mcount = 13680;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Pelican Point") == 1
        bQll1 = [0.71988, 0.00103, 0.00379, -0.69408];
        cdQb = [0.67679, 0.22227, -0.66888, 0.21248];
        bQll2 = [0.719874, -0.00114589, 0.00169798, -0.694101];
        wQb = [0.680305, 0.225758, -0.665297, 0.208802];
        WATSON_name =['SIF_0920_0748623272_632FDR_N0450000SRLC00737_0000LUJ01'];
        mcount = 13668;
        wat_z_dim = 1648;
    elseif strcmp (core_target, "Lefroy Bay") == 1
        bQll1 = [0.0290967, 0.0100956, 0.00925302, 0.999483];
        cdQb = [0.691559, 0.170862, -0.679408, 0.175946];
        bQll2 = [0.0290645, 0.0133957, 0.00898051, 0.999447]	;
        wQb = [0.678889, 0.1411, -0.695587, 0.18804];
        WATSON_name =['SIF_0938_0750226087_000FDR_N0460000SRLC00748_0000LMJ01'];
        mcount = 13620;
        wat_z_dim = 1648;
    end
    
    %-----------------------------
    %DEFINITIONS:

    cdx_COREDRILL = [1, 0, 0];              %Core drill x-axis in Coring Drill frame
    cdy_COREDRILL = [0, 1, 0];              %Core drill y-axis in Coring Drill frame
    cdz_COREDRILL = [0, 0, 1];              %Core drill z-axis in Coring Drill frame

    wy_WATSON = [0, 1, 0];                  %WATSON y-axis in WATSON frame
    wz_WATSON = [0, 0, 1];                  %WATSON z-axis in WATSON frame
    wx_WATSON = [1, 0, 0];                  %WATSON x-axis in WATSON frame
    N_SITE = [1, 0 ,0];                     %Geographic N  (x-axis) in Site frame
    E_SITE = [0, 1, 0];                     %Geographic E  (y-axis) in Site frame
    up_SITE = [0, 0, -1];                   %Geographic Up (-z-axis) in Site frame
    
    %-----------------------------
    %ORIENT CORE HADE AND AZIMUTH:

    bQll1quat = quaternion(bQll1);         %create Rover-Mechanical-to-Site quaternion during time of drilling from entries above (scalar-vector notation)
    cdQbquat = quaternion(cdQb);           %create Coring-Drill-to-Rover-Mechanical quaternion uring time of drilling from entries above (scalar-vector notation)
    cdQllquat =  bQll1quat*cdQbquat;       %combined quaternion rotating from Coring Drill to Site frame (scalar-vector notation)
   
    %Calculate Coring Drill x- and y-axes in SITE frame
    cdx_SITE = rotatepoint(cdQllquat, cdx_COREDRILL);                   %Coring Drill x-axis (pointing vector) in Site frame calculated using combined quaternion
    core_plane_gradient = cross(cross(-cdx_SITE, -up_SITE), -cdx_SITE); %gradient of Core plane: gradient = (normal * down) * normal;
    cdy_SITE = -core_plane_gradient/norm(core_plane_gradient);          %Coring Drill y-axis in Site frame
    
    cdx_projSxSy_SITE = Projection3(-N_SITE', E_SITE', cdx_SITE')';     %projection of Coring Drill x-axis into SITE plane reported in SITE coordinates

    cd_hade = acosd(dot(cdx_SITE,-up_SITE));                                                      %Core hade; Weiss et al. (2023) equation (1)
    cd_azimuth = wrapTo360(signedAngleTwo3DVectors(N_SITE, cdx_projSxSy_SITE, -up_SITE,1)*180/pi); %Core azimuth; Weiss et al. (2023) equation (2)

    %Print methods used for final values in Initial Reports:
    ['Hade of core pointing vector from drill and rover quaternions: ' , num2str(cd_hade), char(176)]
    ['Azimuth of core pointing vector from drill and rover quaternions: ', num2str(cd_azimuth), char(176)]

    %-----------------------------
    %ORIENT WATSON IMAGE FOR CORE ROLL:
    
    bQll2quat = quaternion(bQll2);       %create quaternion during WATSON imaging rotating from Rover Mechanical to Site frame (scalar-vector notation)
    wQbquat = quaternion(wQb);           %create quaternion during WATSON imaging rotating from WATSON to Rover Mechanical frame (scalar-vector notation)

    wQllquat = bQll2quat*wQbquat;                %quaternion rotating from WATSON to Site frame (scalar vector)
    wy_SITE  = rotatepoint(wQllquat,wy_WATSON);  %WATSON y-axis in SITE frame
    wz_SITE  = rotatepoint(wQllquat,wz_WATSON);  %WATSON z-axis in SITE frame
    wx_SITE  = rotatepoint(wQllquat,wx_WATSON);  %WATSON x-axis in SITE frame

    %Calculate core roll as clockwise angle from WATSON y-axis to projection of Coring Drill y-axis into WATSON plane 
    cdy_projWyWz_SITE = Projection3(wy_SITE', wz_SITE', cdy_SITE')';  %projection of Coring Drill y-axis into WATSON plane reported in SITE coordinates
    alpha  = wrapTo360(signedAngleTwo3DVectors(wy_SITE,cdy_projWyWz_SITE,wx_SITE,1)*180/pi); %Core roll; Weiss et al. (2023) equation (3)

    %Calculate direction toward North in WATSON image as clockwise angle from WATSON y-axis to projection of Martian Site north into WATSON plane
    N_projWyWz_SITE = Projection3(wy_SITE', wz_SITE', N_SITE')';  %projection of geographic N into WATSON image plane reported in SITE coordinates
    N_azimuth = wrapTo360(signedAngleTwo3DVectors(wy_SITE, N_projWyWz_SITE, wx_SITE,1)*180/pi); %clockwise angle of N projected into WATSON plane from cy

    ['Core roll (clockwise angle of up-dip in WATSON image plane from WATSON y-axis): ', num2str(alpha), char(176)]
    ['Azimuth of N from WATSON y-axis: ', num2str(N_azimuth), char(176)]

    if markimage == 1
        pscale = WATSON_mark4(WATSON_in_path, WATSON_out_path, WATSON_name, WATSON_out_filetype, mcount, alpha, N_azimuth, wat_z_dim);
        ['WATSON image scale (microns per pixel): ', num2str(pscale)]
    end
end

